#include <wiringPi.h>
#include <stdio.h>

int main()
{
	const int trigger_pin=4;
	const int echo_pin=5;
	const int button_pin=21;

	wiringPiSetup();

	pinMode(echo_pin, INPUT);
	pinMode(trigger_pin, OUTPUT);
	digitalWrite(trigger_pin, LOW);
	pinMode(button_pin, INPUT);

	unsigned int echo_start=0, echo_end=0;
	int input_prev=HIGH;
	int ultra=0;

	while(1){
		int input_cur=digitalRead(button_pin);

		if(input_prev==HIGH && input_cur==LOW)
			ultra=1;

		if(input_prev!=input_cur)
			input_prev=input_cur;

		if(ultra==1){
			digitalWrite(trigger_pin, HIGH);
			delayMicroseconds(10);
			digitalWrite(trigger_pin, LOW);

			while(digitalRead(echo_pin)==LOW)
				echo_start=micros();

			while(digitalRead(echo_pin)==HIGH)
				echo_end=micros();

			double distance=(331.5+0.61*25)*((echo_end-echo_start)/2.0/1000000)*100;

			printf("Distance: %.2f cm\n", distance);
			ultra=0;
		}

		delay(20);
	}

	return 0;
}